/*
 *  v4lcam.c
 *  libCV
 *
 *  Created by Gregory Koch on 11/03/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#include <stdlib.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h> 
#include <sys/mman.h>

#include "v4lcam.h"
#include "ccvt.h"

v4lcam_t v4lcam_open(char* device, unsigned int format)
{
    v4lcam_t cam;
    int i;
	struct video_capability vcap;
	struct video_channel vc;
    
    switch(format)
    {
        case V4L_RGB24_640_480:
            cam.framebuf = init_image(IMG_RGB24, init_size(640, 480));
            break;
        case V4L_GREY8_640_480:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(640, 480));
            break;
        case V4L_RGB24_320_240:
            cam.framebuf = init_image(IMG_RGB24, init_size(320, 240));
            break;
        case V4L_GREY8_320_240:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(320, 240));
            break;
        case V4L_RGB24_160_120:
            cam.framebuf = init_image(IMG_RGB24, init_size(160, 120));
            break;
        case V4L_GREY8_160_120:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(160, 120));
            break;
        default:
            fprintf(stderr, "libCV : v4lcam : unknow format\n");
            exit(EXIT_FAILURE);
            break;
    }
    
    cam.format = VIDEO_PALETTE_YUV420P;
    
    cam.fdv = open(device, O_RDWR);
    
	if(cam.fdv <= 0){
		perror(device);
		exit(1);
	}
    
    if(ioctl(cam.fdv, VIDIOCGCAP, &vcap) < 0){
		perror("VIDIOCGCAP");
		exit(1);
	}
    
	for(i = 0; i < vcap.channels; i++){
		vc.channel = i;
        
		if(ioctl(cam.fdv, VIDIOCGCHAN, &vc) < 0){
			perror("VIDIOCGCHAN");
			exit(1);
		}
	}
    
	vc.channel = CHANNEL;
	vc.norm = VIDEO_MODE_PAL;
    
	if(ioctl(cam.fdv, VIDIOCSCHAN, &vc) < 0){
		perror("VIDIOCGCHAN");
		exit(1);
	}
    
	if(ioctl(cam.fdv, VIDIOCGMBUF, &cam.mbuf) < 0){
		perror("VIDIOCGMBUF");
		exit(1);
	}
    
	cam.buffer = (unsigned char*)mmap(0, cam.mbuf.size, PROT_READ | PROT_WRITE, MAP_SHARED, cam.fdv, 0);
    
	if((int)cam.buffer == -1){
		perror("mmap");
		exit(1);
	}
    
    cam.mm.frame = 0;
	cam.mm.height = cam.framebuf.size.height;
	cam.mm.width = cam.framebuf.size.width;
	cam.mm.format = cam.format;
    
    
    cam.fps = 0;    
    gettimeofday(&cam.tv, NULL);
    
    return cam;
}


void v4lcam_close(v4lcam_t camera)
{
    close(camera.fdv);
    munmap(camera.buffer, camera.mbuf.size);
    free_image(camera.framebuf);
}


void v4lcam_grab_image(v4lcam_t *camera)
{
    struct timeval t;
    
    /* recup du nombre de frame */
	if(ioctl(camera->fdv, VIDIOCMCAPTURE, &(camera->mm)) < 0){
		perror("VIDIOCMCAPTURE");
		exit(1);
	}
	/* attendre que la frame soit complete */
	if(ioctl(camera->fdv, VIDIOCSYNC, &(camera->mm.frame)) < 0){
		perror("VIDIOCSYNC");
		exit(1);
	}
    
    switch(camera->framebuf.type)
    {
        case IMG_RGB24:
            ccvt_420p_rgb24(camera->framebuf.size.width, camera->framebuf.size.height,
                            camera->buffer, camera->framebuf.data);
            break;
        case IMG_GREYSCALE8:
            ccvt_420p_grey8(camera->framebuf.size.width, camera->framebuf.size.height,
                            camera->buffer, camera->framebuf.data);
            break;
        default:
            fprintf(stderr, "libCV : v4lcam : unknow format\n");
            exit(EXIT_FAILURE);
            break;
    }
    
    gettimeofday(&t, NULL);
    camera->fps = 1000000.0 / ( (t.tv_sec - camera->tv.tv_sec)*1000000.0 + (t.tv_usec - camera->tv.tv_usec));
    camera->tv = t;
    
}

